/*
*@功能：ESP32平衡小车
*@作者：刘泽文
*@时间：2020/3/19
*/
#include "mycar.h"
#include "bmp.h"
#include <WiFi.h>
#include <Ticker.h>
#include <hcsr04.h>
#include <U8g2lib.h>
#include <MPU6050_tockn.h>
#include <Wire.h>

//实例化
HCSR04 hcsr04(TRIG_PIN, ECHO_PIN, 20, 4000);
MPU6050 mpu6050(Wire);
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

//mpu6050定时器内更新
//Ticker mpu_update;

void taskOne(void * parameter);
void taskTwo(void * parameter);
void mpu6050_int(void);
void OLED_int(void);
void gpio_int(void);
void mpu6050_update(void);
 
void setup() {
  Serial.begin(9600);

  OLED_int();
  gpio_int();
  mpu6050_int();

  //mpu_update.attach_ms(200, mpu6050_update);

  delay(1000);
  xTaskCreate(
              taskOne,          /*任务函数*/
              "TaskOne",        /*带任务名称的字符串*/
              100000,           /*堆栈大小，单位为字节*/
              NULL,             /*作为任务输入传递的参数*/
              2,                /*任务的优先级*/
              NULL);            /*任务句柄*/
  xTaskCreate(
              taskTwo,          /*任务函数*/
              "TaskTwo",        /*带任务名称的字符串*/
              100000,            /*堆栈大小，单位为字节*/
              NULL,             /*作为任务输入传递的参数*/
              1,                /*任务的优先级*/
              NULL);            /*任务句柄*/
}
 
void loop(){
  delay(100);
  mpu6050_update();
  LED_PWM;
}

void taskOne(void * parameter){
  while(1){
    Encoder_refresh_time = millis();
    while((millis()-Encoder_refresh_time)<Encoder_sample_time)//在采样时间内对编码器计数(四倍频)
    {
      if(LEFT_A==HIGH && left_a_flag==0){
        left_a_val++;
        left_a_flag=1;
        }
      if(LEFT_A==LOW && left_a_flag==1){
        left_a_val++;
        left_a_flag=0;
        }
      if(LEFT_B==HIGH && left_b_flag==0){
        left_b_val++;
        left_b_flag=1;
        }
      if(LEFT_B==LOW && left_b_flag==1){
        left_b_val++;
        left_b_flag=0;
        }
    }
    Encoder_refresh_time = millis();
    while((millis()-Encoder_refresh_time)<Encoder_sample_time)//在采样时间内对编码器计数(四倍频)
    {
      if(RIGHT_A==HIGH && right_a_flag==0){
        right_a_val++;
        right_a_flag=1;
        }
      if(RIGHT_A==LOW && right_a_flag==1){
        right_a_val++;
        right_a_flag=0;
        }
      if(RIGHT_B==HIGH && right_b_flag==0){
        right_b_val++;
        right_b_flag=1;
        }
      if(RIGHT_B==LOW && right_b_flag==1){
        right_b_val++;
        right_b_flag=0;
        }
    }

    left_speed=(left_a_val+left_b_val)/(Encoder_sample_time/1000);//计算转速
    right_speed=(right_a_val+right_b_val)/(Encoder_sample_time/1000);//计算转速
    //清零储存脉冲数的变量
    left_a_val=0;left_b_val=0;
    right_a_val=0;right_b_val=0;

    delay(100);
  }
  Serial.println("Ending task 1");
  vTaskDelete( NULL );
}
 
void taskTwo(void * parameter){
  while(1){
    delay(100);
    u8g2.firstPage();
    do{
      u8g2.setFont(u8g2_font_wqy14_t_gb2312a);
      car_distance = float(hcsr04.distanceInMillimeters())/10;
      u8g2.setCursor(0,16-1);
      u8g2.print("距离:");
      u8g2.setCursor(48,16-1);
      u8g2.print(car_distance);
      u8g2.drawStr(88,16-1,"cm");

      u8g2.setCursor(0,16*2-1);
      u8g2.print("左轮速度:");
      u8g2.setCursor(72,16*2-1);
      u8g2.print(left_speed);
      u8g2.drawStr(107,16*2-1,"rad");

      u8g2.setCursor(0,16*3-1);
      u8g2.print("右轮速度:");
      u8g2.setCursor(72,16*3-1);
      u8g2.print(right_speed);
      u8g2.drawStr(107,16*3-1,"rad");

      u8g2.setCursor(0,16*4-1);
      u8g2.print("电源电压:");
      u8g2.setCursor(72,16*4-1);
      u8g2.print((((analogRead(POWER_ADC_PIN)*3.3)/4095)*910)/138.6);
      u8g2.drawStr(104,16*4-1,"V");

      u8g2.drawCircle(64-((64*MPU6050_NUM.angleX)/180),32-((32*MPU6050_NUM.angleY)/180),3);
    }while(u8g2.nextPage());
  }
  Serial.println("Ending task 2");
  vTaskDelete( NULL );
}

/*
*@初始化mpu6050
*/
void mpu6050_int(void){
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
}

/*
*@初始化OLED屏
*/
void OLED_int(void){
  u8g2.begin();
  u8g2.enableUTF8Print();
  u8g2.setDisplayRotation(U8G2_R0);//设置显示器方向
  u8g2.firstPage();
  do{
    u8g2.setFont(u8g2_font_wqy14_t_gb2312a);
    u8g2.setCursor(16,16*3-8);
    u8g2.print("ESP32平衡小车");
  }while (u8g2.nextPage());
}

/*
*@初始化用到的所有IO口
*/
void gpio_int(void){
  //灯和蜂鸣器
  pinMode(LED_PIN,OUTPUT);
  LED_OFF;
  pinMode(BEEP_PIN,OUTPUT);
  BEEP_ON;
  delay(200);
  BEEP_OFF;
  //霍尔编码器
  pinMode(LEFT_A_PIN,INPUT);
  pinMode(LEFT_B_PIN,INPUT);
  pinMode(RIGHT_A_PIN,INPUT);
  pinMode(RIGHT_B_PIN,INPUT);
  //电源ADC
  adcAttachPin(POWER_ADC_PIN);//将引脚连接到ADC
  adcStart(POWER_ADC_PIN);//在连接的引脚总线上开始ADC转换
  analogReadResolution(12);//设置aliogRead返回值的分辨率2^12
  //电机PWM
  ledcSetup(channel1, freq1, resolution1); // 设置通道1
  ledcAttachPin(LEFT_PWM_1, channel1);  // 将通道0与引脚19连接
  ledcSetup(channel2, freq2, resolution2); // 设置通道2
  ledcAttachPin(LEFT_PWM_2, channel2);  // 将通道0与引脚19连接
  ledcSetup(channel3, freq3, resolution3); // 设置通道3
  ledcAttachPin(RIGHT_PWM_1, channel3);  // 将通道0与引脚19连接
  ledcSetup(channel4, freq4, resolution4); // 设置通道4
  ledcAttachPin(RIGHT_PWM_2, channel4);  // 将通道0与引脚19连接
  //按键引脚,全部内部上拉
  pinMode(KEYU,INPUT_PULLUP);
  pinMode(KEYM,INPUT_PULLUP);
  pinMode(KEYD,INPUT_PULLUP);

  ledcWrite(channel1, 1023);  // 输出PWM
  ledcWrite(channel2, 0);  // 输出PWM
  ledcWrite(channel3, 0);  // 输出PWM
  ledcWrite(channel4, 1023);  // 输出PWM
  delay(1000);
  ledcWrite(channel1, 0);  // 输出PWM
  ledcWrite(channel2, 1023);  // 输出PWM
  ledcWrite(channel3, 1023);  // 输出PWM
  ledcWrite(channel4, 0);  // 输出PWM
}

/*
*@更新mpu6050的角度值到本地结构体
*/
void mpu6050_update(void){
  mpu6050.update();
  MPU6050_NUM.angleX = mpu6050.getAngleX();
  MPU6050_NUM.angleY = mpu6050.getAngleY();
  MPU6050_NUM.angleZ = mpu6050.getAngleZ();
  MPU6050_NUM.temp = mpu6050.getTemp();
}